#include "StdAfx.h"
#include "Measure.h"
#include "Model.h"

CModel::CModel(void)
  : type_(kShape)
{
  Init();
}


CModel::CModel(const tstring& name, int type)
  : name_(name)
  , type_(type)
{
  Init();
}


CModel::~CModel(void)
{
}


void CModel::Init(void)
{
  handle_         = kInvalidHandle;
  score_          = 0;
  find_x_         = 0;
  find_y_         = 0;
  find_angle_     = 0;

  roi_gravity_x_      = 0;
  roi_gravity_y_      = 0;
  roi_gravity_x_back_ = 0;
  roi_gravity_y_back_ = 0;

  num_levels_     = kAutoParam;
  angle_start_    = 0;
  angle_extent_   = 0;
  angle_step_     = 3;
  optimization_   = kAutoParam;
  metric_         = kAutoParam;
  contrast_low_   = 20;
  contrast_high_  = 30;
  min_size_       = 8;
  min_contrast_   = 10;

  min_score_      = 700;
  num_matches_    = 1;
  max_overlap_    = 0;
  sub_pixel_      = kAutoParam;
  greediness_     = 500;

  roi_components_.clear();
  roi_operators_.clear();

  angle_box_2 ab2(0, 0, 15, 15, 0);
  roi_components_.push_back(ab2);
  roi_operators_.push_back(0);
}

void CModel::Reset(void)
{
  score_        = 0;
  find_x_       = 0;
  find_y_       = 0;
  find_angle_   = 0;
}

void CModel::Close(void)
{
  if (handle_ == kInvalidHandle) {
    return;
  }

  switch (type_)
  {
  case kShape:
    Halcon::clear_shape_model(handle_);
    break;

  case kNCC:
    Halcon::clear_ncc_model(handle_);
    break;

  default :
    break;
  }

  handle_ = kInvalidHandle;
}

long CModel::Create(void)
{
  return CErrorCode::kUnimplemented;
}

void CModel::Write(void)
{
  if (handle_ == kInvalidHandle) {
    return;
  }

  switch (type_)
  {
  case kShape:
    Halcon::write_shape_model(handle_, path_name().c_str());
    break;

  case kNCC:
    Halcon::write_ncc_model(handle_, path_name().c_str());
    break;

  default :
    break;
  }
}

void CModel::Read(void)
{
  Close();

  switch (type_)
  {
  case kShape:
    Halcon::read_shape_model(path_name().c_str(), &handle_);
    break;

  case kNCC:
    Halcon::read_ncc_model(path_name().c_str(), &handle_);
    break;

  default :
    break;
  }
}

long CModel::Find(void)
{
  Reset();

  Hlong herror = H_MSG_FALSE;
  Halcon::HTuple row, col, angle, score;

  if (type_ == kShape) {
    herror = Halcon::find_shape_model(theImage, handle_, RADIAN(angle_start_),
      RADIAN(angle_extent_), min_score_/1000.0, num_matches_, max_overlap_/1000,
      "least_squares", 0, greediness_/1000, &row, &col, &angle, &score);
  } else {
    herror = Halcon::find_ncc_model(theImage, handle_, RADIAN(angle_start_),
      RADIAN(angle_extent_), min_score_/1000.0, num_matches_, max_overlap_/1000,
      "true", 0, &row, &col, &angle, &score);
  };

  if (H_MSG_TRUE != herror) {
    return herror;
  }

  if (row.Num() != 1) {
    return CErrorCode::kModelInstanceNotFound;
  }

  find_y_     = row[0].D();
  find_x_     = col[0].D();
  find_angle_ = angle_math::normalize(DEGREE(angle[0].D()));
  score_      = vroundp<int>(score[0].D()*1000);

  Halcon::Hobject roiRgn;
  GenerateRoi(&roiRgn);

  double gx = roi_gravity_x_;
  double gy = roi_gravity_y_;
  CMeasureStation *ms = theApp.project().FindStation(station_);
  ASSERT(ms!=NULL);

  Halcon::vector_angle_to_rigid(gy, gx, /*RADIAN(ms->template_coord_sys().OA())*/0,
    find_y_, find_x_, RADIAN(find_angle_), &transform_mat_);
  Halcon::affine_trans_region(roiRgn, &roiRgn, transform_mat_, "false");

  Halcon::set_line_width(hHalconWnd, 1);
  Halcon::set_color(hHalconWnd, "blue");
  Halcon::set_draw(hHalconWnd, "margin");
  Halcon::disp_obj(roiRgn, hHalconWnd);

  return CErrorCode::kOk;
}

tstring CModel::path_name(void) const
{
  std::string path_name = theApp.solution().active_project_directory();

  if (path_name.empty()) {
    return path_name;
  }

  path_name += '\\';
  path_name += name_;

  switch (type_)
  {
  case kShape:
    path_name += ".shm";
    break;

  case kNCC:
    path_name += ".ncm";
    break;

  default :
    break;
  }

  return path_name;
}

void CModel::roi_add(const shape_2& shape2, int opt)
{
  roi_components_.push_back(shape2);
  roi_operators_.push_back(opt);
}

void CModel::roi_clear(void)
{
  roi_components_.clear();
  roi_operators_.clear();
}

void CModel::roi_backup(void)
{
  roi_gravity_x_back_   = roi_gravity_x_;
  roi_gravity_y_back_   = roi_gravity_y_;
  roi_components_back_  = roi_components_;
  roi_operators_back_   = roi_operators_;
}

void CModel::roi_restore(void)
{
  roi_gravity_x_  = roi_gravity_x_back_;
  roi_gravity_y_  = roi_gravity_y_back_;
  roi_components_ = roi_components_back_;
  roi_operators_  = roi_operators_back_;
}

void CModel::GenerateRoi(Halcon::Hobject *roiRgn)
{
  CMeasureStation *ms = theApp.project().FindStation(station_);
  ASSERT(ms!=NULL);

  Halcon::Hobject tmpRgn;
  Halcon::gen_empty_region(roiRgn);
  GenRgnVisitor grv(tmpRgn, ms->template_coord_sys());

  for (std::vector<shape_2>::size_type n=0; n<roi_components_.size(); ++n) {
    boost::apply_visitor(grv, roi_components_[n]);
    switch (roi_operators_[n])
    {
    case kSetUnion:
      Halcon::union2(*roiRgn, tmpRgn, roiRgn);
      break;

    case kSetIntersection:
      Halcon::intersection(*roiRgn, tmpRgn, roiRgn);
      break;

    case kSetDifference:
      Halcon::difference(*roiRgn, tmpRgn, roiRgn);
      break;

    case kSetSymmetricDifference:
      Halcon::symm_difference(*roiRgn, tmpRgn, roiRgn);
      break;

    default:
      break;
    }
  }

  Hlong roiArea = 0;
  Halcon::area_center(*roiRgn, &roiArea, &roi_gravity_y_, &roi_gravity_x_);
}

void CModel::ChangeCoordSys(const CCoordSys& ocs, const CCoordSys& ncs)
{
  ChangeCSVisitor ccsv(ocs, ncs);
  for (std::vector<shape_2>::size_type n=0; n<roi_components_.size(); ++n) {
    boost::apply_visitor(ccsv, roi_components_[n]);
  }
  ocs.Inverse(roi_gravity_x_, roi_gravity_y_);
  ncs.Transform(roi_gravity_x_, roi_gravity_y_);
}